/*
 * World.cpp
 *
 *  Created on: Jul 13, 2009
 *      Author: scott
 */
#include <allegro.h>
#include "World.h"
#include <irrlicht/vector3d.h>

#include "MeshObject.h"
#include "Broom.h"

World::World(SceneGraphNode * root,SAMPLE * sample) {
	// initialize ode
	dInitODE2(0);
	world = dWorldCreate();
	space = dHashSpaceCreate(0);
	contactGroup = dJointGroupCreate(0);
	dWorldSetGravity (world,0,-9.8 ,0);
	//dWorldSetCFM(world, 1e-5);
	dWorldSetERP(world, 1);
	//dWorldSetLinearDamping(world, 0.00001);
	//dWorldSetAngularDamping(world, 0.005);
	//dWorldSetMaxAngularSpeed(world, 200);
	//dGeomID ground = dCreatePlane(space,0,1,0,-1);
	dCreatePlane(space,0,1,0,-1);
	//dWorldSetAutoDisableFlag(world, 1);
	//dWorldSetAutoDisableAverageSamplesCount(world, 10);
	//dWorldSetContactMaxCorrectingVel(world, 0.1);
	//dWorldSetContactSurfaceLayer(world, 0.001);

	MeshObject * floor = new MeshObject(root, "media/floor3.3ds");
	floor->setTranslation(irr::core::vector3df(-10.0f, -1.2f, 10.0f));
	floor->setRotation(irr::core::vector3df(-90.0f, 0.0f, 0.0f));
	floor->setScale(irr::core::vector3df(2.0f, 2.0f, 1.0f));
	this->sample = sample;
}

World::~World() {
	// TODO Auto-generated destructor stub
}

void World::update()
{
	dSpaceCollide(space,(void*)this,&staticCollisionCallback);
	dWorldStep(world,0.01);
	dJointGroupEmpty(contactGroup);
}

float * World::ode2Opengl(float* M, const float* p, const float* R)
{
    M[0]  = R[0]; M[1]  = R[4]; M[2]  = R[8];  M[3]  = 0;
    M[4]  = R[1]; M[5]  = R[5]; M[6]  = R[9];  M[7]  = 0;
    M[8]  = R[2]; M[9]  = R[6]; M[10] = R[10]; M[11] = 0;
    M[12] = p[0]; M[13] = p[1]; M[14] = p[2];  M[15] = 1;
    return M;
}

float * World::opengl2OdeR(float* R, const float* M)
{
    R[0] = M[0]; R[4] = M[1]; R[8] = M[2]; R[3] = 0;
    R[1] = M[4]; R[5] = M[5]; R[9] = M[6]; R[7] = 0;
    R[2] = M[8]; R[6] = M[9]; R[10] = M[10]; R[11] = 0;
    return R;
}

float * World::opengl2OdeP(float * P, const float * M)
{
	P[0] = M[12]; P[1] = M[13]; P[2] = M[14];
	return P;
}

void World::staticCollisionCallback(void * data, dGeomID o1, dGeomID o2)
{
	((World*) data)->collisionCallback(o1, o2);
}

void World::collisionCallback(dGeomID o1, dGeomID o2)
{
  int i;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  long data1 = 0;
  long data2 = 0;
  if (b1)
	  data1 = (long) dBodyGetData(b1);
  if (b2)
	  data2 = (long) dBodyGetData(b2);

  if ((data1 == 1 && data2 == 2) || (data1 == 2 && data2 == 1))
  {
	  stop_sample(sample);
	  play_sample(sample, 64,128,1000, FALSE);
  }


#define MAX_CONTACTS 8

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
			   sizeof(dContact))) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world,contactGroup,contact+i);
      dJointAttach (c,b1,b2);
    }
  }

}

